// -*- c -*-

#use "libsim.ic"

//This is a tag that the simulator is in use
#define USESIMULATOR 1

int sim_servo, sim_servo0, sim_servo1, sim_servo2, sim_servo3;

/////////////////////////////////////////////////////////////
// Button Functions

// Button bit mask definitions
#define A_BTN   0x0001
#define B_BTN   0x0002
#define RIGHT_BTN  0x0010
#define LEFT_BTN  0x0020
#define UP_BTN   0x0040
#define DOWN_BTN  0x0080
#define R_BTN   0x0100
#define L_BTN   0x0200

// ALL_BTNS is a mask including all the buttons that IC has access to.
// This is equivalent to : (A_BTN | B_BTN | RIGHT_BTN | LEFT_BTN |
// UP_BTN | DOWN_BTN | R_BTN | L_BTN)
#define ALL_BTNS  0x03F3

// The following are aliases to provide a uniform interface across
// disparate targets for choosing and escaping from menus
#define CHOOSE_BTN A_BTN
#define ESCAPE_BTN B_BTN

// button_state returns a bit vector of the state of all the buttons.
// To tell if a particular button is pressed you bitwise and the
// result of this function withe one of the bit mask definitions for
// each button listed above.  A 0 means a given button is pressed, and
// non-zero means that it is not.  The check_button function makes
// this more convenient by doing the and and inverting the result so
// that you get a 1 if pressed and and 0 if not.
int button_state()
{
    int bit_field = 0;
    int i = 0;
    for(i = 0; i < 10; ++i)
      {
        if(peek2(0x7036+(i*2))) bit_field = bit_field & ~(1 << i);
        else bit_field = bit_field | (1<<i);
    }
    return bit_field;
}

int check_button(int button) 
{
    return(!(button_state() & button));
}

int a_button()
{
    return check_button(A_BTN);
}

int b_button()
{
    return check_button(B_BTN);
}

int up_button()
{
    return check_button(UP_BTN);
}

int down_button()
{
    return check_button(DOWN_BTN);
}

int right_button()
{
    return check_button(RIGHT_BTN);
}

int left_button()
{
    return check_button(LEFT_BTN);
}

int r_button()
{
    return check_button(R_BTN);
}

int l_button()
{
    return check_button(L_BTN);
}

int any_button()
{
    return !((button_state() & ALL_BTNS) == ALL_BTNS);
}

// The following are aliases to provide a uniform interface across
// disparate targets for choosing and escaping from menus
int choose_button()
{
    return a_button();
}
int escape_button()
{
    return b_button();
}


// The following functions take a button mask and two button states,
// previous and current, and returns whether the state of the
// requested button did the following:
//   transit:   state changed, either by being hit or released
//   hit:       was not pressed previously, but is currently
//   held:      was pressed during both states
//   released:  was pressed previously, but is not currently

int button_transit(int button, int prev_state, int curr_state)
{
    return ((curr_state ^ prev_state) & button);
}

int button_hit(int button, int prev_state, int curr_state)
{
    return ((~curr_state & prev_state) & button);
}

int button_held(int button, int prev_state, int curr_state)
{
    return (~(curr_state | prev_state) & button);
}

int button_released(int button, int prev_state, int curr_state)
{
    return  (curr_state & ~prev_state) & button;
}


/////////////////////////////////////////////////////////////
// Display Functions

void display_clear() 
{
    callml(101, 0);
}

void display_set_xy(int x, int y) 
{
    callml(200, x, y);
}

void display_get_xy(int *x, int *y) 
{
    callml(201, (int)x, (int)y);
}

/////////////////////////////////////////////////////////////
// Sensor Functions
int digital(int port)
{
    if(port < 16 && port > 7) return sim_sensor(port);
    printf("Digital must be between 8 and 15\n");
    return -1;
}

int extra_digital(int port)
{
    if(port < 24 && port > 15) return sim_sensor(port);
    printf("Extra digital must be between 15 and 24\n");
    return -1;
}

int analog12(int port)
{
    if(port < 8 && port >= 0) return sim_sensor(port);
    printf("Analog sensors must be between 0 and 7\n");
    return -1;
}

// 8-bit analog for HB compatibility
int analog(int port)
{
    if(port < 8 && port >= 0) return (sim_sensor(port)/16);
    printf("Analog sensors must be between 0 and 7\n");
    return -1;
}

void sim_set_analog(int port, int value)
{
	if(port >= 8 || port < 0) 
	{
		printf("Analog sensors must be between 0 and 7\n");
		return;
	}
	callml(2200, port, value*16);
}

void sim_set_analog12(int port, int value)
{
	if(port >= 8 || port < 0)
	{
		printf("Analog sensors must be between 0 and 7\n");
		return;
	}
	callml(2200, port, value);
}

void sim_set_digital(int port, int value)
{
	if(!(port < 16 && port > 7))
	{
		printf("Digital sensors must be between 8 and 15\n");
		return;
	}
	callml(2200, port, value);
}

/////////////////////////////////////////////////////////////
// Servo Functions

void enable_servos()
{
    sim_servo = 1;
}

void disable_servos()
{
    sim_servo = 0;
}

void set_servo_position(int servo, int pos)
{
    if(servo < 0 || servo > 3)
      {
        printf("Servo index must be between 0 and 3\n");
        return;
    }
    if (pos < 0 || pos > 255)
      {
        printf("Servo position must be between 0 and 255\n");
        return;
    }
    if(!sim_servo) return;
    if(servo == 0) sim_servo0 = pos;    
    if(servo == 1) sim_servo1 = pos;
    if(servo == 2) sim_servo2 = pos;
    if(servo == 3) sim_servo3 = pos;
}

int get_servo_position(int servo)
{
    
    if(servo < 0 || servo > 3)
      {
        printf("Servo index must be between 0 and 3\n");
        return -1;
    }
    if(!sim_servo) return 0;
    if(servo == 0) return sim_servo0;    
    if(servo == 1) return sim_servo1;
    if(servo == 2) return sim_servo2;
    if(servo == 3) return sim_servo3;
}

/////////////////////////////////////////////////////////////
// Motor Functions

//////////////////////////
// XBC-specific motor functions

// WARNING: These only work with firmware version 1.04 or later

// Recommended default acceleration for motor position and velocity
// commands.  Value used can be changed by calling
// set_motion_acceleration.  The actual default is set by the firmware
// on bootup.  Use get_motion_acceleration to determine what value is
// currenlty being used
#define DEFAULT_MOTION_ACCEL  5000

// Acceleration used by the freeze command
#define FREEZE_ACCEL  5000

// Maximum allowable velocity
#define MAX_VEL   1000

long get_motor_position_counter(int motor)
{
    return 0L;
}

void set_motor_position_counter(int motor, long value)
{
}

void clear_motor_position_counter(int motor)
{
}

void move_at_velocity(int motor, int velocity)
{
}


// Move motor to goal_pos at given velocity.  The amount actually
// moved will be goal_pos - get_motor_position().
void move_to_position(int motor, int velocity, long goal_pos)
{ 
}

// Move delta_pos relative to the current position of the motor.  The
// final motor tick will be get_motor_position() at the time of the
// call + delta_pos.
void move_relative_position(int motor, int velocity, long delta_pos)
{ 
}

void mrp(int motor, int velocity, long delta_pos)
{
}

void block_motor_done(int motor)
{
}

void bmd(int motor)
{
	block_motor_done(motor);
}

void mav(int motor, int velocity)
{
	move_at_velocity(motor, velocity);
}

void mav_x(int motor, int velocity, int gain_index)
{
}

void mrp_x(int motor, int velocity, long position, int gain_index)
{
}

void mtp(int motor, int velocity, long position)
{
}

void mtp_x(int motor, int velocity, long position, int gain_index)
{
}


// Actively hold motor at current position
void freeze(int motor)
{ 
}

void set_pid_gains(int x, int y, int z)
{
}

void set_motion_acceleration(int motor, int acceleration)
{
}

int get_motion_acceleration(int motor)
{
    return 0;
}
//TODO
int get_motor_done(int motor)
{
    return 0;
}
//TODO
void setpwm(int motor, int pwm)
{
	sim_motor(motor, pwm, 0, 3);
}

void set_array_gains(int array_index)
{
}

//////////////////////////
// Classic motor functions, reimplemented in terms of the above
/***************************************************************** */
/*                                                                */
/* MOTOR PRIMITIVES                                               */
/*                                                                */
/*   fd(n) sets motor n to full on in the green direction    */
/*   bk(n) sets motor n to full on in the red direction      */
/*   motor(n, s) sets motor n on at speed s;               */
/*     s= 100 is full on green,                  */
/*     s= -100 is full on red,                   */
/*     s= 0 is off                               */
/*   off(n)  turns off motor n      */
/*                                                                */
/*   alloff() turns off all motors                      */
/*   ao()  turns off all motors                      */
/*                                                                */
/*                                                                */
/*   motors are numbered 0 through 3.                          */
/***************************************************************** */

void fd(int motor)
{
    setpwm(motor, 100);
}

void bk(int motor)
{ 
    setpwm(motor, -100);
}

void motor(int port, int speed)
{
    
    if(speed > 100) {
        speed = 100;
    }
    else if(speed < -100) {
          speed = -100;
      }
    setpwm(port, speed);
    
}

void off(int motor)
{   
    setpwm(motor, 0); 
}

void alloff()
{
	int i = 0;	
	for(i = 0; i < 4; ++i)
	{
	   off(i);
	}
}

void ao()
{
	alloff();
}

//////////////////////////
// Encoder functions (HB compatible)
// Since there are 16 digital ports, 
// users can actually have 16 encoders.
// For botball, we chop the top 8 off so
// we can have compatible sensors for all
// boards.
// To enable encoders for all 16 ports,
// make each function body look like this:
// if(port <= 16 && !!port)
// {
//     return callml(<func#>, port);
// }

//TODO
int enable_encoder(int port)
{
    /*
    if(port > 7 && port < 16)
      {
        return callml(170, (port-8)*2);
    }
    printf("Encoder port must be between 8 and 15\n");
    return 0;
*/
    return 0;    
}

//TODO
void disable_encoder(int port)
{
    /*
    if(port > 7 && port < 16)
      {
        callml(171, (port-8)*2);
        return;
    }
    printf("Encoder port must be between 8 and 15\n");
    */
}

//TODO
void reset_encoder(int port)
{
    /*
    if(port > 7 && port < 16)
      {
        callml(172, (port-8)*2);
        return;
    }
    printf("Encoder port must be between 8 and 15\n");
*/
    
}

//TODO
int read_encoder(int port)
{
    /*
    if(port > 7 && port < 16)
      {
        return callml(173, (port-8)*2);
    }
    printf("Encoder port must be between 8 and 15\n");
*/    
    return 0;
}

//TODO
int enable_extra_encoder(int port)
{
    /*
    if(port > 15 && port < 24)
      {
        return callml(170, ((port-16)*2)+1);
    }
    printf("Extra encoder port must be between 16 and 23\n");
*/    
    return 0;
}

//TODO
void disable_extra_encoder(int port)
{
    /*
    if(port > 15 && port < 24)
      {
        callml(171, ((port-16)*2)+1);
        return;
    }
    printf("Extra encoder port must be between 16 and 23\n");
*/    
}

//TODO
void reset_extra_encoder(int port)
{
    /*
    if(port > 15 && port < 24)
      {
        callml(172, ((port-16)*2)+1);
        return;
    }
*/    
    printf("Extra encoder port must be between 16 and 23\n");
}

//TODO
int read_extra_encoder(int port)
{
    /*
    if(port > 15 && port < 24)
      {
        return callml(173, ((port-16)*2)+1);
    }
    printf("Extra encoder port must be between 16 and 23\n");
*/    
    return 0;
}

//////////////////////////
// Sonar functions (HB compatible)
// This is a blocking sonar call,
// just like the handyboard version.
// The port number is the lower of the
// two digital ports used for the sonar
// sensor, and will always be used as the 
// input.
// The call will block until the reply
// happens, or up to 50ms, which ever
// happens first. The callml function
// simply returns the number of 
// microseconds for the round trip of
// the signal. 
// This is passed to the scaling function
// scale_sonar, which then turns the value
// into something similar to the handyboard
// value.

int scale_sonar(int microsecs)
{
    int mm;
    mm=(microsecs/100)*17;
    if(mm>4000 || mm<0) mm=32767;
    return mm;
    return 0;    
}

int sonar(int port)
{
    if(port > 7 && port < 16) {
        return peek2(0x704A);
    }        
    printf("Sonar port must be between 8 and 15\n");
    return 0;    
}

//////////////////////////
// Sonar functions (XBC specific)
// This is a bank of non-blocking 
// sonar functions for the XBC.

int set_sonar_port(int port)
{
    if(port > 7 && port < 16)  return 1;
    printf("Sonar port must be between 8 and 15\n");
    return 0;    
}

void send_sonar_ping(int port)
{
}

//TODO
int get_sonar_ping_time(int port)
{
    if(port > 7 && port < 16) return sonar(port);
    printf("Sonar port must be between 8 and 15\n");
    return 0;    
}

int get_scaled_sonar_value(int port)
{
    if(port > 7 && port < 16) return sonar(port);
    printf("Sonar port must be between 8 and 15\n");
    return 0;    
}

//////////////////////////
// Battery power functions

//TODO    
float power_level()
{
    /*
    float p;
    callml(102, (int)&p);
    return p;
*/    
}

//////////////////////////
// Random functions

//TODO
int random(int x)
{
    return callml(103, x);
}

//TODO
void seed_random(long seed)
{
    callml(102, (int)&seed);
}

void set_digital_output_value(int port, int value)
{
}

void set_green_led(int status)
{
}

void set_red_led(int status)
{
}

void display_clear_at(int x, int y, int width)
{
 callml(301, x, y, width);
}


// For emacs to interpret formatting uniformly despite dotfile differences:
//   Local variables:
//    comment-column: 40
//    c-indent-level: 4
//    c-basic-offset: 4
//   End:
